%     File:  BabybootWithDAlembertMethod.txt
%  Problem:  Analysis of 3D chaotic double pendulum.
% Copyright (c) 2009 Motion Genesis LLC.  All rights reserved.
%----------------------------------------------------------------
%----------------------------------------------------------------

NewtonianFrame N
RigidBody      A        % Upper rod
RigidBody      B        % Lower plate
Point 		C(B)
%----------------------------------------------------------------

Variable   qA''         % Pendulum angle and its time-derivatives
Variable   qB''         % Plate angle and its time-derivative
Specified  TA

Constant   LA = 7.5 cm  % Distance from pivot to A's mass center
Constant   LB = 20 cm   % Distance from pivot to B's mass center
Constant   LC
A.SetMassInertia( mA =  10 grams,  IAx = 50 g*cm^2,  IAy,  IAz )
B.SetMassInertia( mB = 100 grams,  IBx = 2500 g*cm^2, IBy = 500 g*cm^2, IBz = 2000 g*cm^2 )
%----------------------------------------------------------------

%       Rotational and translational kinematics.
A.RotateX( N, qA )
B.RotateZ( A, qB )

% Check manual expression
express(alf_B_N> - (qA''*nx> + qb''*az> - qA'*qB'*ay>),N)

Acm.Translate( No, -LA*Az> )
Bcm.Translate( No, -LB*Az> )
C.Translate( No, -LB*Az> + LC*by> )
%----------------------------------------------------------------

%       Add relevant forces.
g> = -9.81*Nz>
System.AddForceGravity( g> )
%----------------------------------------------------------------

%       Form equations of motion (angular momentum principle).
Dynamics[1] = Dot( Bz>,  B.GetDynamics(Bcm)  )
Dynamics[2] = Dot( Ax>,  System(A,B).GetDynamics(No)  )
Solve( Dynamics = 0,  qA'',  qB'' )
%----------------------------------------------------------------

%       Kinetic and potential energy.
KE = System.GetKineticEnergy()
PE = System.GetForceGravityPotentialEnergy( g>, No )
Energy = KE + PE
%----------------------------------------------------------------

%       Integration parameters and initial values.
Input  tFinal = 10 sec,  tStep = 0.02 sec,  absError = 1.0E-07,  relError = 1.0E-07    
Input  qA = 45 deg,  qA' = 0.0 rad/sec,  qB = 1 deg,  qB' = 0.0 rad/sec 
%----------------------------------------------------------------

%       List output quantities and solve ODEs.
nzBcm = Dot(nz>, Bcm.GetPosition(No))
nyBcm = Dot(ny>, Bcm.GetPosition(No))
OutputPlot  t sec,  qB deg
OutputPlot  t sec, Energy N*m
OutputPlot  nyBcm, nzBcm
% OutputPlot  t sec,  qA deg,  qB deg,  Energy N*m
% Output t sec,  qA deg,  qB deg,  Energy N*m
ODE()  Babyboot.m
%----------------------------------------------------------------

% Save BabybootWithDAlembertMethod.all
% Quit
quit